//
// SoftServ -- Servo Serial Controller with software PWM
//
// 2008, Tod E. Kurt, http://todbot.com/blog/
//
//
// Responds to two different command sets: 
// "MiniSSC modified" and "text mode"
// 
// MiniSSC modified:
// -- 3 byte protocol,
// -- 1st byte is always 0xFF
// -- 2nd byte is servo number:  0x00 - 0x07, or alternate command set
// -- 3rd byte is servo position: 0x00 - 0xFE 
// -- the 'alternate command' for the 2nd & 3rd bytes can be one of:
// -- -- 0xFE 0x00 -- change input mode to text mode
// -- -- 0xFD 0x00 -- dump status
// -- -- 0xFC 0x00 -- turn servos off
// -- -- 0xFC 0x01 -- turn servos on
// -- -- 0xFB 0xnn -- set slew rate of all servos to 0xnn
//
// Text mode:
// -- all ascii text, first character is command type
// -- sNNpp    -- set servo number NN to position pp
// -- rNNpp    -- set servo number NN's slew rate to pp
// -- m0 or m1 -- turn servos off or on
// -- i        -- change input mode to MiniSSC mode
// -- d        -- dump status
//
//


#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <util/delay.h>
#include <stdlib.h>  // for abs()
#include <ctype.h>

#define DEBUG 0

#define BAUD 9600 

#define UBBRVAL (((F_CPU/16)/BAUD)-1)

// Servo locations. note we're using all 8 pins of PORTB
#define SERVO_DDR   DDRB
#define SERVO_PORT  PORTB

#define SERVO_NUM       8     // number of sevos

#define SERVO_MINPOS    0     // servo min. position
#define SERVO_MAXPOS    255   // servo max. position

#define SERVO_MINTIME   500   // min pulsewidth, in microseconds
#define SERVO_MAXTIME  2500   // max pulsewidth in microseconds

#define SERVO_DEFAULT_RATE 10

#define SERVO_USECS_PER_POS ((SERVO_MAXTIME-SERVO_MINTIME)/(SERVO_MAXPOS-SERVO_MINPOS))/2

// datastructure for servos
typedef struct {
    uint8_t on;   // boolean on/off
    uint8_t pos;  // current position (0-255)
    uint8_t destpos; // destination position
    //uint8_t min;  // minimum pulse in usec
    //uint8_t max;  // maximum pulse in usec
    uint8_t rate; // rate to go from pos to destpos
} servo;

volatile servo servos[SERVO_NUM];

#define BUFF_SIZE     8     // serial input buffer size
uint8_t cmdbuf[BUFF_SIZE];   // serial buffer
uint8_t cmdi;                // pointer into cmdbuf
uint8_t moving=1; // drive servos or not: 1==yes, 0==no
uint8_t ovfcnt;   // counts number of timer0 overflows
#define ovfcntmax 9        // max overflows needed to hit 50Hz

uint8_t inputmode;         // 0 = text, 1 = MiniSSC
#define TEXT_INPUT 0
#define MINISSC_INPUT 1

// take a ascii hex character, return the nibble value for it
uint8_t parseHex(char c) 
{
    c = toupper(c);
    if (c >= '0' && c <= '9') return (c - '0');
    if (c >= 'A' && c <= 'F') return (c - 'A')+10;
    return 0;
}
// given a number, return the ascii hex character for it
char toHex(uint8_t n)
{
    n += 0x30;             // add offset to ASCII '0'
    if (n > '9') n += 7;  // if alpha add offset to ASCII 'A'
    return n;
}

// send a character out the serial port
void uartPutChar(uint8_t c)
{
    while( ! (UCSRA & (1<<UDRE) ) ) ;
    UDR = c;
}
void uartPutNewline(void) 
{
    uartPutChar('\r');
    uartPutChar('\n');
}
void uartPutHex(uint8_t b)
{
    uartPutChar(toHex(b>>4)); uartPutChar(toHex(b&0x0f));
}
// send a string out the serial port
void uartPutstr(uint8_t* str, uint8_t nl)
{
    while( *str )
        uartPutChar( *str++ );
    if( nl )
        uartPutNewline();
}
// send a string from ROM out the serial port
#define uartPutstrP(x,y) uartPutstrROM(PSTR(x),y)
void uartPutstrROM(const char *str, uint8_t nl)
{
    uint8_t c;
    while((c = pgm_read_byte(str++)))
        uartPutChar(c);
    if( nl ) 
        uartPutNewline();
}
// print out state of things
static void dumpStatus(void) 
{
    uartPutstrP("status dump: inputmode=",0); uartPutHex(inputmode);
    uartPutstrP(", moving=",0); uartPutHex(moving);
    uartPutNewline();
    for( int i=0; i< SERVO_NUM; i++ ) {
        uartPutChar('s');  uartPutHex(i);
        uartPutstrP(" p",0); uartPutHex( servos[i].pos );
        uartPutstrP(" r",0); uartPutHex( servos[i].rate );
        uartPutstrP(" d",0); uartPutHex( servos[i].destpos );
        uartPutNewline();
    }
    uartPutNewline();
}

//
static void handle_commands(void)
{
    uint8_t cmd = cmdbuf[0];
    if( inputmode == TEXT_INPUT ) {   // text mode is easy for humans
        if( cmd == 'd' ) {
            dumpStatus();
        }
        else if( cmd == 'm' ) {
            moving = !moving;
            uartPutChar('m');  uartPutChar( (moving)? '1':'0');
            uartPutNewline();
        }
        else if( cmd == 'i' ) {
            uartPutstrP("going into MiniSSC mode",1);
            inputmode = MINISSC_INPUT;
        }
        else if( cmd == 'r' || cmd == 's' ) { // set pos or rate
            if( cmdi<5 ) return;  // not enough args
            uint8_t num = (parseHex(cmdbuf[1])<<4) + parseHex(cmdbuf[2]);
            uint8_t arg = (parseHex(cmdbuf[3])<<4) + parseHex(cmdbuf[4]);
            if( num < SERVO_NUM && arg != 0xFF ) {
                if( cmd == 's' ) {
                    servos[num].destpos = arg;
                    uartPutChar('s'); 
                } else if( cmd == 'r' ) {
                    servos[num].rate = arg;
                    uartPutChar('r'); 
                }
                uartPutHex( num );
                uartPutChar(' ');  uartPutHex( arg );
                uartPutNewline();
            }
        }
    }
    else if( inputmode == MINISSC_INPUT ) {   // MiniSSC mode is faster
        if( cmd == 0xFF ) {                   // MiniSSC mode, 3-bytes always
            if( cmdi<3 ) return;              // not enough args
            uint8_t num = cmdbuf[1];
            uint8_t arg = cmdbuf[2];
            if( num == 0xFE ) {               // change input mode to text
                inputmode = TEXT_INPUT;
            }
            else if( num == 0xFD ) {          // dump status
                dumpStatus();
            }
            else if( num == 0xFC ) {          // set motors on/off
                moving = arg;
            }
            else if( num == 0xFB ) {          // set slew rate
                for( int i=0; i< SERVO_NUM; i++ ) 
                    servos[i].rate = arg;
            }
            else if( num < SERVO_NUM && arg != 0xFF ) {
                servos[num].destpos = arg;
#if DEBUG
                uartPutChar('s');  uartPutHex(num);
                uartPutChar(' ');  uartPutHex(arg);
                uartPutNewline();
#endif
            }
        } // cmd==0xFF
    } 
    else { 
        // alternate other input mode perhaps
    }

    cmdi=0; // say we ate the buffer up, yum
}

// receive chars from serial port
SIGNAL(SIG_USART0_RECV)
{
    if( cmdi == BUFF_SIZE ) {
        cmdi=0;
        uartPutChar('!');
        return; // oops ran out of buffer
    }
    cmdbuf[cmdi++] = UDR;
    handle_commands();
}
//
// Every time Timer0 overflows (clk/1024/256 = 30Hz),
// this routine gets called.  It's the start-of-frame for servo PWM.
//
ISR(TIMER0_OVF_vect)
{
    if( ovfcnt++ != ovfcntmax )
        return;
    ovfcnt=0;

    if( !moving ) 
        return;

    // slew servos towards destination pos
    for( uint8_t i=0; i< SERVO_NUM; i++ ) {
        volatile servo* serv = &servos[i];
        int16_t diff = (uint8_t)serv->pos - (uint8_t)serv->destpos;
        if( diff == 0 ) continue;
        else if( diff > -serv->rate ) serv->pos++;
        else if( diff < serv->rate ) serv->pos--;
        if( diff > 0 ) serv->pos -= serv->rate;
        else if( diff < 0 ) serv->pos += serv->rate;
    }

    // If servo is enabled, start a pulse
    for( uint8_t i=0; i< SERVO_NUM; i++ ) {
        if( servos[i].on ) SERVO_PORT |= (1<<i);
    }
    _delay_us( SERVO_MINTIME );  // minimum pulsewidth for everyone
    
    // now extend pulses for each servo, then turn them off
    for( uint16_t p=SERVO_MINPOS ; p<=SERVO_MAXPOS ; p++) {
        /*// for some reason this doesn't work
        for( uint8_t i=0; i<SERVO_NUM; i++ ) {
            if(servos[i].pos < p) SERVO_PORT &=~ (1<<i);
        } */
        if(servos[0].pos < p) SERVO_PORT &=~ (1<<0);
        if(servos[1].pos < p) SERVO_PORT &=~ (1<<1);
        if(servos[2].pos < p) SERVO_PORT &=~ (1<<2);
        if(servos[3].pos < p) SERVO_PORT &=~ (1<<3);
        if(servos[4].pos < p) SERVO_PORT &=~ (1<<4);
        if(servos[5].pos < p) SERVO_PORT &=~ (1<<5);
        if(servos[6].pos < p) SERVO_PORT &=~ (1<<6);
        if(servos[7].pos < p) SERVO_PORT &=~ (1<<7);
        // a delay would normally go here, to be the usec resolution
        // of the positioning, with a value of SERVO_USECS_PER_POS.
        // But the overhead of above if-statements is about the right
        // amount of time at 8MHz so we do nothing
        //_delay_us( SERVO_USECS_PER_POS );  // resolution is x usec/pos 
    }
    SERVO_PORT = 0;  // turn off everyone
}

//
int main(void)
{
    DDRD  |= (1<<PD3);
    PORTD |= (1<<PD3);  // LED

    SERVO_DDR  = 0xFF;   // set all pins to output
    SERVO_PORT = 0x00;   // set all outputs low
    
    // initialize servo structs
    for( int i=0; i< SERVO_NUM; i++ ) {
        servos[i].pos     = (SERVO_MAXPOS-SERVO_MINPOS)/2;
        servos[i].destpos = (SERVO_MAXPOS-SERVO_MINPOS)/2;
        servos[i].rate    = SERVO_DEFAULT_RATE;
        servos[i].on = 1;
    }
    
    // initialize timer for servo pwm
    //TCCR0B = (1<<CS02) | (1<<CS00); // = 8MHz/1024 (/256 ovf) = 30Hz
    //TCCR0B = (1<<CS02);             // = 8MHz/256 (/256 ovf) = 122Hz
    TCCR0B = (1<<CS01) | (1<<CS00);   // = 8MHz/64 (/256 ovf) = 488Hz
    TIMSK = (1<<TOIE0);

    // initialize uart
    UBRRH = UBBRVAL >>8;
    UBRRL = UBBRVAL & 0xff; // baud rate 9600
    // Enable UART receiver and transmitter, and RX interrupt
    UCSRB = (1<<RXCIE) | (1 << RXEN) | (1 << TXEN);
    // Set frame format: async, 8-bit, no parity, 1 stop
    UCSRC = (1<<UCSZ1)|(1<<UCSZ0);
    
    sei();
    
    uartPutstrP("\r\nsoftserv ready for action",1);

    // Loop forever
    while(1) {
        PORTD ^=  (1<<PD3); // toggle LED
        _delay_ms(50);
    }

}



